perm filename CAMERA.SAI[VIS,HPM]1 blob
sn#150657 filedate 1975-03-15 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00006 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00002 00002 BEGIN "CAMERA"
C00005 00003 SIMPLE PROCEDURE TRANS (INTEGER SIZE REAL ARRAY A,B,C)
C00009 00004 PROCEDURE RMAT
C00014 00005 ⊃ Initialization
C00018 00006 ⊃ Iterative solution
C00022 ENDMK
C⊗;
BEGIN "CAMERA"
REQUIRE "PROLOG.HDR[001,MJH]" SOURCE_FILE;
REQUIRE "SFILES.HDR[001,MJH]" SOURCE_FILE;
⊃ Least-squares solution for relative position and orientation
of two camera views of series of points;
REAL ARRAY A,AA1,AA2,B,BB1,BB2,BB3[1:3,1:3],XYF1[1:3],E[1:2],P[1:5,1:2],
G,GP,SP,C,D[1:5],H,S,T[1:5,1:5],X1,X2,Y1,Y2[1:100];
REAL U,V,W,F1,F2,FW,FW2,R,R3,U2,V2,UV,S0,Q,CRIT,DPR,XH1,YH1,XH2,YH2,TEMP;
STRING ARRAY PARAM[1:5]; STRING FNAME,TMPSTR;
INTEGER S1,S2,S3,NO,N,NN,M,ITER,IMAX,I,J,K,PNT,OBS,BRKCHR;
BOOLEAN CONV; LABEL FINISH;
SIMPLE STRING PROCEDURE CAG (INTEGER N; REAL ARRAY A);
⊃ CAG converts a one-dimensional array A of size N to a string
suitable for output in G format;
BEGIN
STRING S;
S←NULL;
FOR I←1 STEP 1 UNTIL N DO S←S&CVG(A[I]);
RETURN(S);
END;
SIMPLE PROCEDURE OUTG2 (REAL ARRAY A);
⊃ OUTG2 displays the two-dimensional array A of size S1 by S2 in G format;
BEGIN
FOR I←1 STEP 1 UNTIL S1 DO
BEGIN
FOR J←1 STEP 1 UNTIL S2 DO OUTSTR(CVG(A[I,J]));
OUTSTR(CRLF);
END;
OUTSTR(CRLF);
END;
SIMPLE PROCEDURE MULT (REAL ARRAY A,B,C);
⊃ MULT forms the matrix product A(S1xS3) = B(S1xS2) * C(S2xS3);
FOR I←1 STEP 1 UNTIL S1 DO FOR K←1 STEP 1 UNTIL S3 DO
BEGIN
A[I,K]←0;
FOR J←1 STEP 1 UNTIL S2 DO A[I,K]←A[I,K]+B[I,J]*C[J,K];
END;
SIMPLE PROCEDURE TRANS (INTEGER SIZE; REAL ARRAY A,B,C);
⊃ TRANS forms the matrix product A = B * C, where A and C are
SIZE-dimensional vectors and B is a square matrix;
FOR I←1 STEP 1 UNTIL SIZE DO
BEGIN
A[I]←0;
FOR J←1 STEP 1 UNTIL SIZE DO A[I]←A[I]+B[I,J]*C[J];
END;
PROCEDURE INVERT (REAL ARRAY A,B);
⊃ INVERT inverts the matrix B to obtain the matrix A (MxM);
BEGIN
REAL T;
REAL ARRAY SCALE[1:M];
FOR I←1 STEP 1 UNTIL M DO SCALE[I] ← 1./SQRT(B[I,I]);
FOR I←1 STEP 1 UNTIL M DO FOR J←1 STEP 1 UNTIL M DO
A[I,J] ← B[I,J]*SCALE[I]*SCALE[J];
FOR I←1 STEP 1 UNTIL M DO
BEGIN
IF A[I,I]≤0 THEN
BEGIN
OUTSTR("ERROR IN MATRIX INVERSION" & CRLF);
GO TO FINISH;
END;
T←1./A[I,I];
A[I,I]←1.;
FOR J←1 STEP 1 UNTIL M DO A[I,J]←T*A[I,J];
FOR K←1 STEP 1 UNTIL M DO IF I≠K THEN
BEGIN
T←A[K,I];
A[K,I]←0;
FOR J←1 STEP 1 UNTIL M DO A[K,J] ← A[K,J]-T*A[I,J];
END
END;
FOR I←1 STEP 1 UNTIL M DO FOR J←1 STEP 1 UNTIL M DO
A[I,J] ← A[I,J]*SCALE[I]*SCALE[J];
END;
SIMPLE PROCEDURE ROTMAT (REAL ARRAY R,DR; REAL ANGLE; INTEGER AXIS);
⊃ ROTMAT computes a rotation matrix R and its derivative DR
for rotating by ANGLE radians about AXIS (1,2,3 for X,Y,Z).
Positive AXIS denotes that the positive direction of rotation
is such that an axis is rotated toward its successor in the
cyclic order X,Y,Z;
BEGIN
REAL C,S;
INTEGER AX;
AX←ABS(AXIS);
C←COS(ANGLE);
S←SIN(ANGLE);
FOR I←1 STEP 1 UNTIL 3 DO FOR J←1 STEP 1 UNTIL 3 DO
BEGIN
R[I,J]←0;
DR[I,J]←0;
END;
I ← (AX MOD 3) + 1; J ← (I MOD 3) + 1;
IF AXIS<0 THEN I↔J;
R[AX,AX]←1.;
R[I,I]←C; DR[I,I]←-S;
R[J,J]←C; DR[J,J]←-S;
R[I,J]←S; DR[I,J]←C;
R[J,I]←-S; DR[J,I]←-C
END;
PROCEDURE RMAT;
⊃ RMAT computes the matrices A = A1*A2 and B = B3*B2*B1 and their
derivatives, by using the values of the parameters (G);
BEGIN
REAL ARRAY A1,A2,B1,B2,B3,A1P,A2P,B1P,B2P,B3P,T[1:3,1:3];
S1←S2←S3←3;
ROTMAT(A1,A1P,G[1],-2); ROTMAT(A2,A2P,G[2],1); ROTMAT(B1,B1P,G[3],2);
ROTMAT(B2,B2P,G[4],-1); ROTMAT(B3,B3P,G[5],-3);
MULT(A,A1,A2); MULT(AA1,A1P,A2); MULT(AA2,A1,A2P);
MULT(T,B2,B1); MULT(B,B3,T); MULT(BB3,B3P,T);
MULT(T,B2P,B1); MULT(BB2,B3,T); MULT(T,B2,B1P); MULT(BB1,B3,T);
END;
PROCEDURE DXY0 (REFERENCE REAL DXDB,DYDB; REAL ARRAY B);
⊃ DXY0 computes derivative of infinity point X0,Y0;
BEGIN
REAL ARRAY UVW[1:3];
TRANS(3,UVW,B,XYF1);
DXDB ← FW*UVW[1] - FW2*U*UVW[3]; DYDB ← FW*UVW[2] - FW2*V*UVW[3];
END;
PROCEDURE DIRN (REFERENCE REAL U,V; REAL ARRAY A,B);
⊃ DIRN computes direction numbers;
BEGIN
REAL ARRAY T1,T2[1:3];
T2[1] ← XYF1[2]*A[3,3] - XYF1[3]*A[2,3];
T2[2] ← XYF1[3]*A[1,3] - XYF1[1]*A[3,3];
T2[3] ← XYF1[1]*A[2,3] - XYF1[2]*A[1,3];
TRANS(3,T1,B,T2); U ← -T1[2]; V ← T1[1];
END;
SIMPLE PROCEDURE DCXCY (REFERENCE REAL DCX,DCY; REAL ARRAY A,B);
⊃ DCXCY computes derivatives of direction cosines;
BEGIN
REAL DU,DV;
DIRN(DU,DV,A,B); DCX←(V2*DU-UV*DV)/R3; DCY←(U2*DV-UV*DU)/R3;
END;
PROCEDURE MODEL (REAL X1,Y1,X2,Y2);
⊃ MODEL uses the current values of the rotation matrices to compute
the discrepancy E (and its partial derivatives P) between the
point X2,Y2 and the point X1,Y1 transformed to a line in the
camera 2 plane. NO indicates whether this consists of 1 or 2
observations;
BEGIN
REAL ARRAY DX0,DY0,DCX,DCY[1:5],UVW[1:3]; REAL X0,Y0,CX,CY,XD,YD;
XYF1[1]←X1; XYF1[2]←Y1; XYF1[3]←F1; TRANS(3,UVW,B,XYF1);
U←UVW[1]; V←UVW[2]; W←UVW[3]; FW←F2/W; FW2←FW/W; X0←FW*U; Y0←FW*V;
DX0[1]←DX0[2]←DY0[1]←DY0[2]←0;
DXY0(DX0[3],DY0[3],BB1); DXY0(DX0[4],DY0[4],BB2); DXY0(DX0[5],DY0[5],BB3);
DIRN(U,V,A,B); R ← SQRT(U↑2 + V↑2); CX←U/R; CY←V/R;
R3←R↑3; U2←U↑2; V2←V↑2; UV←U*V; XD←X2-X0; YD←Y2-Y0;
IF XD*CX+YD*CY>0. THEN
BEGIN
NO←1;
DCXCY(DCX[1],DCY[1],AA1,B);
DCXCY(DCX[2],DCY[2],AA2,B); DCXCY(DCX[3],DCY[3],A,BB1);
DCXCY(DCX[4],DCY[4],A,BB2); DCXCY(DCX[5],DCY[5],A,BB3);
E[1] ← YD*CX-XD*CY;
FOR I←1 STEP 1 UNTIL 5 DO
P[I,1] ← YD*DCX[I]-XD*DCY[I]-CX*DY0[I]+CY*DX0[I];
END
ELSE
BEGIN
NO←2; E[1]←XD; E[2]←YD;
FOR I←1 STEP 1 UNTIL 5 DO
BEGIN
P[I,1] ← -DX0[I];
P[I,2] ← -DY0[I];
END;
END;
END;
⊃ Initialization;
M←5; DPR←57.2957795; SETFORMAT(13,8);
PARAM[1] ← "ALPHA 1"; PARAM[2] ← "ALPHA 2";
PARAM[3] ← "BETA 1 "; PARAM[4] ← "BETA 2 "; PARAM[5] ← "BETA 3 ";
WHILE TRUE DO
BEGIN ⊃ Input;
F1←INREAL(CRLF&" INPUT"&CRLF&"FOCAL LENGTH OF CAMERA 1 ←");
IF (F2←INREAL("FOCAL LENGTH OF CAMERA 2 ="&CVF(F1)&"←"))=0 THEN F2←F1;
IF (K←INREAL(CRLF & "TYPE: 0 TO TYPE IN A PRIORI VALUES OF PARAMETERS"&
CRLF & " 1 TO USE BUILT-IN VALUES WITH CAMERA 2 ON RIGHT,"&
CRLF & " -1 TO USE BUILT-IN VALUES WITH CAMERA 2 ON LEFT:"))=0 THEN
BEGIN
OUTSTR(CRLF & "INITIAL APPROXIMATIONS IN DEGREES" & CRLF);
FOR I←1 STEP 1 UNTIL M DO
BEGIN
GP[I]←INREAL(CRLF & PARAM[I] & " ←");
SP[I]←INREAL("STD. DEV. ←");
END;
END
ELSE
BEGIN
FOR I←1 STEP 1 UNTIL M DO BEGIN GP[I]←0;
SP[I]←1000.;
END;
IF K<0 THEN GP[1]←-90. ELSE GP[1]←90.
END;
⊃ S0←INREAL(CRLF & "STD. DEV. OF OBSERVATIONS (NEG. IF TO BE ADJUSTED) ←");
⊃ IF (CRIT←INREAL("CONVERGENCE CRITERION (RADIANS)=1.@-6)←"))=0 THEN CRIT←1.@-6;
S0←-1; CRIT←1.@-6; XH1←YH1←150;
IF (IMAX←ININT("MAXIMUM NUMBER OF ITERATIONS = 20 ←"))=0 THEN IMAX←20;
IF (TEMP←INREAL("SIZE OF FRAME 1 IN X ="&CVF(XH1)&" ←"))≠0 THEN XH1←TEMP;
IF (TEMP←INREAL("SIZE OF FRAME 1 IN Y ="&CVF(YH1)&" ←"))≠0 THEN YH1←TEMP;
XH2←XH1; YH2←YH1;
IF (TEMP←INREAL("SIZE OF FRAME 2 IN X ="&CVF(XH2)&" ←"))≠0 THEN XH2←TEMP;
IF (TEMP←INREAL("SIZE OF FRAME 2 IN Y ="&CVF(YH2)&" ←"))≠0 THEN YH2←TEMP;
XH1←XH1/2; YH1←YH1/2; XH2←XH2/2; YH2←YH2/2;
WHILE INFILE(1,FNAME←STRIN(CRLF&"FILENAME OF INPUT FILM PLANE DATA←"))DO ;
SETBREAK(2,CR,LF,"IN");
I←0;
WHILE (TEMP←REALSCAN(TMPSTR←INPUT(1,2),0))≥0 DO
BEGIN
X1[I←I+1]←TEMP-XH1;
Y1[I]←YH1-REALSCAN(TMPSTR,0);
X2[I]←REALSCAN(TMPSTR,0)-XH2;
Y2[I]←YH2-REALSCAN(TMPSTR,0);
END;
N←I;
⊃ Convert input;
FOR I←1 STEP 1 UNTIL M DO
BEGIN
G[I] ← GP[I] ← GP[I]/DPR; SP[I] ← (DPR*S0/SP[I])↑2;
END;
⊃ Iterative solution;
SETFORMAT(12,5); ITER←0;
OUTSTR(CRLF & "OUTPUT" & CRLF);
OUTSTR(CRLF & "NEG. OF CORRECTIONS ON EACH ITERATION (RADIANS)" & CRLF & CRLF);
DO
BEGIN
IF ITER=IMAX THEN
BEGIN
OUTSTR("CONVERGENCE CRITERION NOT MET" & CRLF);
GO TO FINISH
END;
ITER←ITER+1; RMAT; Q←NN←0;
FOR I←1 STEP 1 UNTIL M DO FOR J←1 STEP 1 UNTIL M DO H[I,J]←0;
FOR I←1 STEP 1 UNTIL M DO
BEGIN
H[I,I] ← SP[I]; C[I] ← SP[I]*(G[I]-GP[I]);
END;
FOR PNT←1 STEP 1 UNTIL N DO
BEGIN
MODEL(X1[PNT],Y1[PNT],X2[PNT],Y2[PNT]); NN←NN+NO;
FOR OBS←1 STEP 1 UNTIL NO DO
BEGIN
Q ← Q + E[OBS]↑2;
FOR I←1 STEP 1 UNTIL M DO
BEGIN
C[I] ← C[I] + P[I,OBS]*E[OBS];
FOR J←1 STEP 1 UNTIL M DO
H[I,J] ← H[I,J] + P[I,OBS]*P[J,OBS];
END;
END;
END;
INVERT(S,H); TRANS(M,D,S,C); CONV←TRUE;
FOR I←1 STEP 1 UNTIL M DO
BEGIN
G[I]←G[I]-D[I]; CONV ← CONV ∧ (ABS D[I]<CRIT);
END;
OUTSTR(CAG(M,D) & CRLF);
END UNTIL CONV;
⊃ Final computations and output;
FINISH: SETFORMAT(16,7); S1←S2←S3←M; MULT(T,S,H);
FOR I←1 STEP 1 UNTIL M DO T[I,I]←T[I,I]-1.;
W←0;
FOR I←1 STEP 1 UNTIL M DO FOR J←1 STEP 1 UNTIL M DO
IF ABS T[I,J] > W THEN W ← ABS T[I,J];
OUTSTR(CRLF & "NUMBER OF ITERATIONS =" & CVS(ITER) & CRLF);
OUTSTR("ACCURACY OF MATRIX INVERSION =" & CVE(W) & CRLF);
IF NN>5 THEN Q←Q/(NN-5) ELSE Q←0;
OUTSTR("EST. STD. DEV. OF OBSERVATIONS =" & CVF(SQRT(Q)) & CRLF);
IF S0>0 THEN Q←S0↑2;
FOR I←1 STEP 1 UNTIL M DO FOR J←1 STEP 1 UNTIL M DO S[I,J] ← Q*S[I,J];
FOR I←1 STEP 1 UNTIL M DO SP[I]←SQRT(S[I,I]);
OUTSTR(CRLF&" ADJUSTED PARAMETER STD. DEV. (DEGREES)"&CRLF);
FOR I←1 STEP 1 UNTIL M DO OUTSTR(PARAM[I]&CVF(DPR*G[I])&CVF(DPR*SP[I])&CRLF);
IF INREAL(CRLF&"OUTPUT RESIDUALS? ≤0 FOR NO, >0 FOR YES:")>0 THEN
BEGIN
RMAT;
FOR PNT←1 STEP 1 UNTIL N DO
BEGIN
MODEL(X1[PNT],Y1[PNT],X2[PNT],Y2[PNT]);
OUTSTR(CRLF&CAG(NO,E));
END;
END;
IF (FNAME←STRIN("OUTPUT FILE NAME=______.CDT (NULL→NO OUTPUT) ←")) THEN
BEGIN
OUTFILE(1,FNAME&".CDT");
OUT(1,CVF(F1)&CRLF&CVF(F2/F1)&CRLF);
FOR I←1 STEP 1 UNTIL M DO OUT(1,CVF(DPR*G[I])&CRLF);
RELEASE(1);
END;
OUTSTR(CRLF & CRLF);
END;
END "CAMERA"